from math import *

DEGREES_PER_RADIANS = 180.0 / pi
RADIANS_PER_DEGREES = pi / 180.0
PI_OVER_2 = pi / 2.0
RADIUS = 6378137.0
RADIUS_2 = RADIUS * 0.5
RAD_RAD = RADIANS_PER_DEGREES * RADIUS


def lat_to_mercatory(lat):
    rad = lat * RADIANS_PER_DEGREES
    sinnum = sin(rad)
    return RADIUS_2 * log((1.0 + sinnum) / (1.0 - sinnum))


def lon_to_mercatorx(lon):
    return lon * RAD_RAD


def mercatorx_to_lon(x):
    return mercatorx_to_lon_linear(x, True)


def mercatorx_to_lon_linear(x, linear):
    rad = x / RADIUS
    deg = rad * DEGREES_PER_RADIANS
    if linear:
        return deg
    rotations = floor((deg + 180.0) / 360.0)
    return deg - (rotations * 360.0)


def mercatory_to_lat(y):
    rad = PI_OVER_2 - (2.0 * atan(exp(-1.0 * y / RADIUS)))
    return rad * DEGREES_PER_RADIANS


def distance_between_coors(lon1, lat1, lon2, lat2):
    EARTH_RADIUS = 6378137
    radLat1 = lat1 * pi / 180.0
    radLat2 = lat2 * pi / 180.0
    a = radLat1 - radLat2
    b = lon1 * pi / 180.0 - lon2 * pi / 180.0
    s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2)))
    s = s * EARTH_RADIUS
    return s


def util_calc_ref_pos(lon_1, lat_1, lon_2, lat_2):
    """
    :param lon_1:
    :param lat_1:
    :param lon_2:
    :param lat_2:
    :return: 返回两点之间的距离以及两点连线与正北方向夹角
    """
    EARTH_RADIUS = 6378137
    DEG2RAD = pi / 180.0
    ESP = 0.0000001
    if abs(lon_2 - lon_1) < ESP and abs(lat_2 - lat_1) < ESP:
        s = 0
        heading = 0
        return [s, heading]

    l1 = lon_1 * DEG2RAD
    l2 = lon_2 * DEG2RAD
    n1 = lat_1 * DEG2RAD
    n2 = lat_2 * DEG2RAD
    tmp = sin(n1) * sin(n2) + cos(n1) * cos(n2) * cos(l2 - l1)
    if tmp > 1:
        tmp = 1 - ESP
    s = EARTH_RADIUS * acos(tmp)

    if s < ESP * EARTH_RADIUS:
        heading = 0
    else:
        tmp = (cos(n2) * sin(l2 - l1)) / sin(s / EARTH_RADIUS)
        if tmp > 1:
            tmp = 1 - ESP
        heading = asin(tmp) / DEG2RAD
        if lon_2 > lon_1 and lat_2 > lat_1:  # phase 1
            heading = heading
        elif lon_2 > lon_1 and lat_2 <= lat_1:  # phase 2
            heading = 180 - heading
        elif lon_2 <= lon_1 and lat_2 <= lat_1:  # phase 3
            heading = 180 - heading
        elif lon_2 <= lon_1 and lat_2 > lat_1:  # phase 4
            heading = heading + 360
        else:
            heading = 0

    return [s, heading]


def calc_ref_lonlat(lon1, lat1, dpsi, dis):
    """
    根据方位角和距离计算目标点经纬度
    :param lon1: 原点经度 deg
    :param lat1: 原点纬度 deg
    :param dpsi: 方位角 deg
    :param dis:  距离 m
    :return:
    """
    EARTH_RADIUS = 6378137
    DEG2RAD = pi / 180.0
    RAD2DEG = 1 / DEG2RAD
    # ESP = 0.0000001

    dlon = RAD2DEG * dis * sin(dpsi*DEG2RAD) / (EARTH_RADIUS * cos(lat1 * DEG2RAD))
    dlat = RAD2DEG * dis * cos(dpsi*DEG2RAD) / EARTH_RADIUS

    lon2 = lon1 + dlon
    lat2 = lat1 + dlat

    return lon2, lat2


def linelength_between_point(x1, y1, x2, y2):
    return sqrt(((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)))


def intersect_point_with_line_param(a1, b1, c1, a2, b2, c2):
    if (fabs(b2 * a1 - b1 * a2) < 1e-6) or (fabs(a2 * b1 - a1 * b2) < 1e-6):
        return None, None
    x = (b1 * c2 - b2 * c1) / (b2 * a1 - b1 * a2)
    y = (a1 * c2 - a2 * c1) / (a2 * b1 - a1 * b2)
    return x, y


def point_to_line_distance(x, y, a, b, c):
    return fabs((a * x + b * y + c) / sqrt(a * a + b * b))


def vector_point_multi(x1, y1, x2, y2):
    return x1 * x2 + y1 * y2


# 全局的变量，记录轨道信息
line_center_vertical_a = 0  # 轨道顶端边界线的ab
line_center_vertical_b = 0
line_center_vertical_start_c = 0  # 过起点的顶端边界线的c
line_center_vertical_end_c = 0  # 过终点的顶端边界线的c
real_proj_radio = 0  # 跑道附近实际米和投影距离的比例，用于到端点距离的换算
runway_start_x = 0  # 起点投影位置
runway_start_y = 0
runway_end_x = 0  # 终点投影位置
runway_end_y = 0


def cal_runway_param(start_lon, start_lat, end_lon, end_lat, width):
    """
    计算跑道参数
    :param start_lon:起点经度
    :param start_lat:起点纬度
    :param end_lon:终点经度
    :param end_lat:终点纬度
    :param width:跑道宽度
    :return:跑道两边参数，坐标系和起点终点坐标系一致，
    格式为：[(起点A侧经度，起点A侧纬度), (终点A侧经度，终点A侧纬度), (起点B侧经度，起点B侧纬度), (终点B侧经度，终点B侧纬度)]
    """
    global runway_start_x, runway_start_y, runway_end_x, runway_end_y
    runway_start_x = lon_to_mercatorx(start_lon)
    runway_start_y = lat_to_mercatory(start_lat)

    runway_end_x = lon_to_mercatorx(end_lon)
    runway_end_y = lat_to_mercatory(end_lat)

    real_distance = distance_between_coors(start_lon, start_lat, end_lon, end_lat)
    l = linelength_between_point(runway_start_x, runway_start_y, runway_end_x, runway_end_y)

    global real_proj_radio
    real_proj_radio = real_distance / l

    # 跑道一半的宽度
    project_dis = (width * l / real_distance) / 2.0

    # 中心线参数
    line_center_a = runway_start_y - runway_end_y
    line_center_b = runway_end_x - runway_start_x
    line_center_c = -line_center_a * runway_end_x - line_center_b * runway_end_y

    delta = project_dis * sqrt(line_center_a * line_center_a + line_center_b * line_center_b)

    # A侧轨道参数
    line_a_c = line_center_c + delta

    # B侧轨道参数
    line_b_c = line_center_c - delta

    global line_center_vertical_a, line_center_vertical_b, line_center_vertical_start_c, line_center_vertical_end_c
    # 垂线共同参数
    line_center_vertical_a = line_center_b
    line_center_vertical_b = -line_center_a

    # 过起点垂线参数
    line_center_vertical_start_c = -line_center_vertical_a * runway_start_x - line_center_vertical_b * runway_start_y

    # 过终点垂线参数
    line_center_vertical_end_c = -line_center_vertical_a * runway_end_x - line_center_vertical_b * runway_end_y

    # 起点A侧交点
    start_a_x, start_a_y = intersect_point_with_line_param(line_center_vertical_a, line_center_vertical_b,
                                                           line_center_vertical_start_c,
                                                           line_center_a, line_center_b, line_a_c)
    start_a_lon = mercatorx_to_lon(start_a_x)
    start_a_lat = mercatory_to_lat(start_a_y)

    # 起点B侧交点
    start_b_x, start_b_y = intersect_point_with_line_param(line_center_vertical_a, line_center_vertical_b,
                                                           line_center_vertical_start_c,
                                                           line_center_a, line_center_b, line_b_c)
    start_b_lon = mercatorx_to_lon(start_b_x)
    start_b_lat = mercatory_to_lat(start_b_y)

    # 终点A侧交点
    end_a_x, end_a_y = intersect_point_with_line_param(line_center_vertical_a, line_center_vertical_b,
                                                       line_center_vertical_end_c,
                                                       line_center_a, line_center_b, line_a_c)
    end_a_lon = mercatorx_to_lon(end_a_x)
    end_a_lat = mercatory_to_lat(end_a_y)

    # 终点B侧交点
    end_b_x, end_b_y = intersect_point_with_line_param(line_center_vertical_a, line_center_vertical_b,
                                                       line_center_vertical_end_c,
                                                       line_center_a, line_center_b, line_b_c)
    end_b_lon = mercatorx_to_lon(end_b_x)
    end_b_lat = mercatory_to_lat(end_b_y)

    return [[start_a_lon, start_a_lat], [end_a_lon, end_a_lat], [start_b_lon, start_b_lat], [end_b_lon, end_b_lat]]


def util_calc_runway_param(start_lon, start_lat, end_lon, end_lat, width):
    """
    计算跑道参数
    :param start_lon:起点经度
    :param start_lat:起点纬度
    :param end_lon:终点经度
    :param end_lat:终点纬度
    :param width:跑道宽度
    :return:跑道两边参数，坐标系和起点终点坐标系一致，
    格式为：[(起点A侧经度，起点A侧纬度), (终点A侧经度，终点A侧纬度), (起点B侧经度，起点B侧纬度), (终点B侧经度，终点B侧纬度)]
    """
    rwy_s_x = lon_to_mercatorx(start_lon)
    rwy_s_y = lat_to_mercatory(start_lat)

    rwy_e_x = lon_to_mercatorx(end_lon)
    rwy_e_y = lat_to_mercatory(end_lat)

    real_distance = distance_between_coors(start_lon, start_lat, end_lon, end_lat)
    l = linelength_between_point(rwy_s_x, rwy_s_y, rwy_e_x, rwy_e_y)

    # real_proj_radio = real_distance / l

    # 跑道一半的宽度
    project_dis = (width * l / real_distance) / 2.0

    # 中心线参数
    line_center_a = rwy_s_y - rwy_e_y
    line_center_b = rwy_e_x - rwy_s_x
    line_center_c = -line_center_a * rwy_e_x - line_center_b * rwy_e_y

    delta = project_dis * sqrt(line_center_a * line_center_a + line_center_b * line_center_b)

    # A侧轨道参数
    line_a_c = line_center_c + delta

    # B侧轨道参数
    line_b_c = line_center_c - delta

    # 垂线共同参数
    center_v_a = line_center_b
    center_v_b = -line_center_a

    # 过起点垂线参数
    cvs_c = -center_v_a * rwy_s_x - center_v_b * rwy_s_y

    # 过终点垂线参数
    cve_c = -center_v_a * rwy_e_x - center_v_b * rwy_e_y

    # 起点A侧交点
    start_a_x, start_a_y = intersect_point_with_line_param(center_v_a, center_v_b,
                                                           cvs_c,
                                                           line_center_a, line_center_b, line_a_c)
    start_a_lon = mercatorx_to_lon(start_a_x)
    start_a_lat = mercatory_to_lat(start_a_y)

    # 起点B侧交点
    start_b_x, start_b_y = intersect_point_with_line_param(center_v_a, center_v_b,
                                                           cvs_c,
                                                           line_center_a, line_center_b, line_b_c)
    start_b_lon = mercatorx_to_lon(start_b_x)
    start_b_lat = mercatory_to_lat(start_b_y)

    # 终点A侧交点
    end_a_x, end_a_y = intersect_point_with_line_param(center_v_a, center_v_b,
                                                       cve_c,
                                                       line_center_a, line_center_b, line_a_c)
    end_a_lon = mercatorx_to_lon(end_a_x)
    end_a_lat = mercatory_to_lat(end_a_y)

    # 终点B侧交点
    end_b_x, end_b_y = intersect_point_with_line_param(center_v_a, center_v_b,
                                                       cve_c,
                                                       line_center_a, line_center_b, line_b_c)
    end_b_lon = mercatorx_to_lon(end_b_x)
    end_b_lat = mercatory_to_lat(end_b_y)

    return [[start_a_lon, start_a_lat], [end_a_lon, end_a_lat], [start_b_lon, start_b_lat], [end_b_lon, end_b_lat]]


def cal_runway_distance(lon, lat, lon_old, lat_old):
    """
    计算当前经纬度点和角度，到跑道顶端的距离
    :param lat_old:
    :param lon_old:
    :param lon: 飞机经度
    :param lat: 飞机纬度
    :param yaw: 飞机机头朝向
    :return: 到跑道顶端的距离
    """
    if (lon_old is None) or (lat_old is None) or \
            ((line_center_vertical_a == 0) and (line_center_vertical_b == 0)):
        return None
    if (lon_old == lon) and (lat_old == lat):
        return None
    # 生成飞机航行向量
    plane_x = lon_to_mercatorx(lon)
    plane_y = lat_to_mercatory(lat)
    plane_x_old = lon_to_mercatorx(lon_old)
    plane_y_old = lat_to_mercatory(lat_old)
    vec_plane = (plane_x - plane_x_old, plane_y - plane_y_old)

    # 生成飞机和起点的向量
    vec_start = (runway_start_x - plane_x, runway_start_y - plane_y)

    # 生成飞机和终点的向量
    vec_end = (runway_end_x - plane_x, runway_end_y - plane_y)

    # 计算航向向量与飞机位置到跑道起点的向量夹角
    mul_start = vector_point_multi(vec_plane[0], vec_plane[1], vec_start[0], vec_start[1])

    # 计算航向向量与飞机位置到跑道终点的向量夹角
    mul_end = vector_point_multi(vec_plane[0], vec_plane[1], vec_end[0], vec_end[1])

    dis = None
    d_start = point_to_line_distance(plane_x, plane_y, line_center_vertical_a,
                                     line_center_vertical_b, line_center_vertical_start_c)
    d_end = point_to_line_distance(plane_x, plane_y, line_center_vertical_a,
                                   line_center_vertical_b, line_center_vertical_end_c)
    if (mul_start > 0) and (mul_end > 0):
        # 如果夹角同向且夹角都在[-90,90]之间，取最远的距离返回
        if d_start > d_end:
            dis = d_start
        else:
            dis = d_end
    elif mul_start > 0:
        # 如果一个同向一个不同向，计算同向的距离
        dis = d_start
    elif mul_end > 0:
        # 如果一个同向一个不同向，计算同向的距离
        dis = d_end
    else:
        # 冲出轨道边界线了
        if d_start > d_end:
            dis = -d_end
        else:
            dis = -d_start

    if dis is not None:
        dis = dis * real_proj_radio

    return dis


# lon = 113.1089440950
# lat = 32.6967719279
# print(lon_to_mercatorx(lon), lat_to_mercatory(lat))
#
# lon = 123.8315517750
# lat = 45.2729299470
# print(lon_to_mercatorx(lon), lat_to_mercatory(lat))

# s_lon = 118.7622788242
# s_lat = 31.8314690242
# e_lon = 118.7662324003
# e_lat = 31.8315556210
# w = 50
# cal_runway_param(s_lon, s_lat, e_lon, e_lat, w)
# p_lon = 118.7620857051
# p_lat = 31.831254810
# pl_lon = 118.7619891456
# pl_lat = 31.8312046758
# d = cal_runway_distance(p_lon, p_lat, pl_lon, pl_lat)
# print(d)
if __name__ == '__main__':
    lon1 = 118
    lat1 = 31
    lon2 = 117.99
    lat2 = 32.1
    [s, h] = util_calc_ref_pos(lon1, lat1, lon2, lat2)
    print("h = %f" % h)
